g = -9.81;%重力加速度9.81是标准值.m/s^2
n = 100; % 轨迹中的点数

% 球的初始条件
p0 = 0; % 单位m,表示初始高度
v0 = 100;% 向上抛的速度 单位m/s

% 时间
t0 = 0; % 初始时间,单位s
tf = 10; % 时间区间
t = linspace(t0,tf,n);% 时间区间上的点
dt = (tf-t0)/n; % 每个点所占时间间隔

% 实际的高度变化轨迹(实际的观测数据)
pf = p0 + v0*t + 0.5*g*t.*t;
rng('default');
stdev = 20; % 噪声的标准偏差(正态分布平均值=0),randn产生正态分布的随机数或矩阵的函数
% 有噪声的观测数据
noisy = pf + stdev*randn(1,n);

% Kalman filter
A = [1 dt; 0 1]; % 状态转移矩阵,预期轨迹,A * Q_est:p0=p0+v0*dt,v0=v0
B = [dt^2/2; dt]; % 输入控制矩阵，输入加速度的预期效果,B * u:dt^2/2*g,dt*g
C = [1 0]; % 观测矩阵,因为不同变量之间单位不同,这个起一个缩放尺寸的作用

u = g; 
%初始数据
Q = [p0; v0];
%预测数据
Q_est = Q;
acc_noise_mag = 0.05;
sensor_noise_mag = 10;
Ez = sensor_noise_mag^2;%测量数据的方差
%预测数据的方差,Q_est=f(dt/2)二次方;,v=f(dt)一次方
Ex = acc_noise_mag^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2];
P = Ex; % 初始(预测)数据的方差


Q_loc_est = [];
vel_est = [];
P_est = P;
P_mag_est = [];
pred_state = [];
pred_var = [];

for i = 1:n
    % 预测过程
    Q_est = A * Q_est + B * u;
    %pred_state = [pred_state; Q_est(1)];
    P = A * P * A' + Ex;
    %pred_var = [pred_var; P];

    %更新过程
    K = P*C'*inv(C*P*C'+Ez);
    Q_est = Q_est + K * (noisy(i) - C*Q_est);
    P = ( eye(2) - K*C ) * P;
    Q_loc_est = [Q_loc_est; Q_est(1)];
    %vel_est = [vel_est; Q_est(2)];
    %P_mag_est = [P_mag_est; P(1)];
end

% Plotting
plot(t,pf,'r.-',t,noisy,'k.-', t,Q_loc_est,'g.-');
legend('Actual','Noisy','Predicted');